#include "fence.h"


bool Fence::addShape (const f64 *data, u64 size) noexcept {

  if (!data || size < 6 || size & 1)
    return false;

  this->shapes.emplace_back(data, size);
  return true;
}


bool Fence::contains (const f64 *data, u64 size, std::vector<u8> &res) noexcept {

  if (!data || size & 1)
    return false;

  u8 r[4];
  while (size >= 8) {
    f64v4 x = {data[0], data[2], data[4], data[6]};
    f64v4 y = {data[1], data[3], data[5], data[7]};
    this->_contains(r, x, y);
    res.emplace_back(r[0]);
    res.emplace_back(r[1]);
    res.emplace_back(r[2]);
    res.emplace_back(r[3]);
    data += 8;
    size -= 8;
  }

  if (size) {
    size >>= 1;
    f64v4 x, y;
    switch (size) {
      case 1:
        x = _mm256_set_pd(data[0], 0.0, 0.0, 0.0);
        y = _mm256_set_pd(data[1], 0.0, 0.0, 0.0);
        break;
      case 2:
        x = _mm256_set_pd(data[0], data[2], 0.0, 0.0);
        y = _mm256_set_pd(data[1], data[3], 0.0, 0.0);
        break;
      case 3:
        x = _mm256_set_pd(data[0], data[2], data[4], 0.0);
        y = _mm256_set_pd(data[1], data[3], data[5], 0.0);
        break;
      default:
        x = y = _mm256_set1_pd(0.0);
    }
    this->_contains(r, x, y);
    for (auto i = 0; i < size; ++i)
      res.emplace_back(r[i]);
  }
}


void Fence::_contains (u8 result[4], f64v4 x, f64v4 y) noexcept {

  auto _0 = (s64v4)_mm256_set1_epi64x(0L);
  auto _n1 = (s64v4)_mm256_set1_epi64x(-1L);
  auto r = _0;

  for (auto &shape : this->shapes) {
    auto c = shape.contains(x, y);
    c = _mm256_cmpeq_epi64(c, _0);
    c = (s64v4)_mm256_xor_pd((f64v4)c, (f64v4)_n1);
    r = (s64v4)_mm256_or_pd((f64v4)r, (f64v4)c);
  }

  for (auto i = 0; i < 4; ++i)
    result[i] = (u8)(r[i] != 0L);
}